# -*- coding: utf-8 -*-

from math import sqrt
import numpy as np

class aStar:
    
    def __init__(self, img):
        self._START = (192,48)
        self._GOAL = (260,508)
        self._img = img

    def g(self, node, img):
        """
        Returns the movement cost to a particular node.
        """
        return 1 - ((self._img[node[0],node[1]]) / 255.0)
    
    def h(self, node):
        """
        Heuristic function:       
        Return the distance from a particular node to the target node.
        """
        
        return sqrt( ((self._GOAL[0] - node[0])**2) + ((self._GOAL[1] -
            node[1])**2) )

    def f(self, node):
          
        return self.g(node, self._img) + self.h(node) 
    
    def findNeighbours(self, node):
        
        return [(node[0] + x, node[1] + y) for x in xrange(-1,2) for y in xrange(-1,2) if (not (x==0 and y==0))]

    def reconstruct_path(cameFrom, currentNode):
        if currentNode in cameFrom:
            path = reconstruct_path(cameFrom, cameFrom[currentNode])
            return (p + currentNode)
        else:
            return currentNode
                
    def run(self):
        
        closedSet = set()             #The set of nodes already evaluated. 
        openSet = set()               #The set of tentative nodes to evaluate.
        openSet.add(self._START)      
        cameFrom = {}                 #The map of navigated nodes.
        g_cost = {}
        f_cost = {}
        currentNode = self._START   
        i = 0
        while len(openSet) > 0:
            i+=1
            print i
            for neighbour in self.findNeighbours(currentNode):
                openSet.add(neighbour)
            currentNode = min(self.findNeighbours(currentNode), key = lambda node : self.f(node))
            if (currentNode == self._GOAL): 
                return self.reconstruct_path(cameFrom, self._GOAL)
            openSet.remove(currentNode)
            closedSet.add(currentNode)
            for neighbour in self.findNeighbours(currentNode):
                if neighbour in closedSet:
                    continue
                tentative_g_cost = self.g(currentNode, self._img) + (sqrt( ((currentNode[0] - neighbour[0])**2) + ((currentNode[1] - neighbour[1])**2) ))
                print tentative_g_cost
                print sqrt( ((currentNode[0] - neighbour[0])**2) + ((currentNode[1] - neighbour[1])**2)  )
                if ((neighbour not in openSet) or (tentative_g_cost < self.g(self._img, neighbour))):
                    cameFrom[neighbour] = tentative_g_cost
                    g_cost[neighbour] = tentative_g_cost
                    f_cost[neighbour] = f(neighbour)
                    if neighbour not in openSet:
                        openSet.add(neighbour)
        return 1               
                
if __name__ == '__main__':
    pass
